#include "drv_m2006.h"

/** 
 * @brief:    M2006_Init
 * @details:  初始化
 */
void M2006_Init2(motor_m2006_t* motor1, motor_m2006_t* motor2, uint16_t last1, uint16_t last2)
{ 
  motor1->in_id = MOTOR_TX_ID_5;
  motor1->mst_id = MOTOR_RX_ID_5;
  motor1->round_cnt = 0;
  motor1->last_enco = last1;

  motor2->in_id = MOTOR_TX_ID_6;
  motor2->mst_id = MOTOR_RX_ID_6;
  motor2->round_cnt = 0;
  motor2->last_enco = last2;
}

/**
 * 
 */
void M2006_UpdateMotorData(motor_m2006_t* motor, uint16_t now_enco)
{
  int32_t error = now_enco - motor->last_enco;
  if (error < -2000) {  // 正转过1圈
    motor->round_cnt += 1;
  }
  else if (error > 2000) {  // 反转过1圈
    motor->round_cnt -= 1;
  }
  motor->last_enco = now_enco;
}


// uint32_t M2006_CalcTotalEncodedValue(motor_m2006_t* motor_ptr)
// {
//   return  (uint32_t)motor_ptr->encoded_value +
//           (uint32_t)motor_ptr->round_cnt * M2006_ENCODING_RANGE -
//           (uint32_t)motor_ptr->encoded_value_offset;
// }

// float M2006_CalcAngle(motor_m2006_t* motor_ptr)
// {
//   float round_cnt = (float)M2006_CalcTotalEncodedValue(motor_ptr) / (float)M2006_ENCODING_RANGE;
//   return round_cnt * 360.0f / (float)M2006_REDUCTION_RATIO;
// }

// void M2006_PID_CalcAndSend(motor_m2006_t* motor_ptr, algo_pid_t* pid_ptr, float target_angle)
// {
//   float actual_angle = M2006_CalcAngle(motor_ptr);
//   float angle_change = Algo_PID(pid_ptr, actual_angle, target_angle);
//   int16_t given_current_1 = M2006_Float2Int(angle_change, -16384, 16384, -20.0, 20.0);
//   int16_t given_current_2 = M2006_Float2Int(-angle_change, -16384, 16384, -20.0, 20.0);

//   uint8_t data[8];
//   data[0] = given_current_1 >> 8;
// 	data[1] = given_current_1;
// 	data[2] = given_current_2 >> 8;
// 	data[3] = given_current_2;
// 	data[4] = 
// 	data[5] = 
// 	data[6] = 
// 	data[7] = 0;
//   BSP_CAN_SendData(&hcan1, motor_ptr->tx_header, data, 8);
// }



// int16_t M2006_Float2Int(float x, int16_t max, int16_t min, float current_min, float current_max)
// {
//   return 1;
// }

